﻿using Cognex.VisionPro;
using System;
using System.Collections.Generic;
using System.ComponentModel;
using System.Data;
using System.Drawing;
using System.Linq;
using System.Text;
using System.Threading;
using System.Windows.Forms;
using Cognex.VisionPro.CalibFix;



namespace YJYpublicClass
{
    public partial class Frm_相机标定 : Form
    {

        private System.Timers.Timer timer;//定时器一 9点标定定时器
        private System.Timers.Timer timer_Rotation;//定时器一 旋转中心标定定时器

        //都不直接拿全局变量，都是通过外部传进来，这样有助于模块化，公用性     
        //标定模块
        public Cognex.VisionPro.ToolBlock.CogToolBlock CalibBlock = new Cognex.VisionPro.ToolBlock.CogToolBlock();
        //应用模块
       // public Cognex.VisionPro.ToolBlock.CogToolBlock RUNBlock = new Cognex.VisionPro.ToolBlock.CogToolBlock();
        //标定数据存储
        public CalibDataList Cpras = new CalibDataList();

        //标定模块要拆分成以下几个部分，
        //1、取图模块
        //2、获取图像匹配，直接获取像素坐标   
        //3、九点标定部分
        //4、标定后获取标定后的像素坐标
        //5、旋转中心拟合
        private CogAcqFifoTool FIFO_Acq = null;
        private Cognex.VisionPro.ToolBlock.CogToolBlock _GetOrgPix = null;
        private CogCalibNPointToNPointTool Calib_cel = null;  //用于标定的
        private Cognex.VisionPro.ToolBlock.CogToolBlock _GetAfterCalibPix = null;
        private CogFitCircleTool FitCircle = null;
        //全部需要单独传进来，这里做一个对接口，不直接调用全局变量


        //
        public Frm_相机标定()
        {
            InitializeComponent();
        }
        public void DG_Init()
        {
            #region  【调整表格显示】
            DGInit(ref DG_Xpiexl, 10);
            DGInit(ref DG_Ypiexl, 10);
            DGInit(ref DG_Xrobot, 10);
            DGInit(ref DG_Yrobot, 10);
            DGInit(ref DG_FitCPix_X, 9);
            DGInit(ref DG_FitCPix_Y, 9);
            DGInit(ref DG_FitCRob_X, 9);
            DGInit(ref DG_FitCRob_Y, 9);
            DGInit(ref DG_FitCRob_R, 9);

            #endregion 【机械手Y坐标】
            try
            {
                //=============================================读取配置参数===============================================              
                for (int i = 0; i < 9; i++)
                {
                    DG_Xpiexl.Rows[i].Cells[0].Value = Cpras.NCD.Pixel_X[i];
                    DG_Ypiexl.Rows[i].Cells[0].Value = Cpras.NCD.Pixel_Y[i];
                    DG_Xrobot.Rows[i].Cells[0].Value = Cpras.NCD.Robot_X[i];
                    DG_Yrobot.Rows[i].Cells[0].Value = Cpras.NCD.Robot_Y[i];
                    DG_FitCPix_X.Rows[i].Cells[0].Value = Cpras.FCD.Pixel_X[i];
                    DG_FitCPix_Y.Rows[i].Cells[0].Value = Cpras.FCD.Pixel_Y[i];
                    DG_FitCRob_X.Rows[i].Cells[0].Value = Cpras.FCD.Robot_X[i];
                    DG_FitCRob_Y.Rows[i].Cells[0].Value = Cpras.FCD.Robot_Y[i];
                    DG_FitCRob_R.Rows[i].Cells[0].Value = Cpras.FCD.Robot_Y[i];
                }

            }
            catch (System.Exception ex)
            {
                MessageBox.Show("参数读取失败！" + ex.ToString());
            }

        }

        //工具函数，表格初始化
        #region【工具函数】
        private void DGInit(ref DataGridView dg, int RowNum)
        {
            dg.Enabled = true;
            dg.RowCount = RowNum;
            dg.ColumnCount = 1;
            dg.RowHeadersVisible = false;
            dg.ColumnHeadersVisible = false;
            for (int j = 1; j < RowNum + 1; j++)
            {
                dg.Rows[j - 1].Height = DG_Yrobot.Height / 10;
                dg.Columns[0].Width = DG_Yrobot.Width / 1;

            }
        }
        #endregion
        private void Frm_相机标定_Load(object sender, EventArgs e)
        {

            try
            {
                //表格初始化
                DG_Init();
                //其他参数初始化

                tbx_sanpx.Text = Cpras.ACD.NTNCalibAP.x.ToString();
                tbx_sanpy.Text = Cpras.ACD.NTNCalibAP.y.ToString();
                tbx_sanpr.Text = Cpras.ACD.NTNCalibAP.r.ToString();
                tbx_DistenceX.Text = Cpras.ACD.Xinterval.ToString();
                tbx_DistenceY.Text = Cpras.ACD.Yinterval.ToString();
                tbx_centerX.Text = Cpras.BPD.CircleRobotFitBP.x.ToString();
                tbx_centerY.Text = Cpras.BPD.CircleRobotFitBP.y.ToString();
                //旋转半径这里没有用
                //参数显示
                this.SysGDV.SelectedObject = Cpras.BPD;
                //加载VPP
                this.cogToolBlockEditV21.Subject = CalibBlock;
                //初始化图像处理模块
                Calib_cel = CalibBlock.Tools["CogCalibNTN"] as Cognex.VisionPro.CalibFix.CogCalibNPointToNPointTool;
              //  CalibAll = RUNBlock.Tools["CogCalibNTN"] as Cognex.VisionPro.CalibFix.CogCalibNPointToNPointTool;
                //取图             
                FIFO_Acq = CalibBlock.Tools["CogAcq"] as CogAcqFifoTool;
                //模块要赋值呀。
                _GetOrgPix = CalibBlock.Tools["GetPixelOR"] as Cognex.VisionPro.ToolBlock.CogToolBlock;
                _GetAfterCalibPix = CalibBlock.Tools["GetPixelAF"] as Cognex.VisionPro.ToolBlock.CogToolBlock;
                FitCircle = CalibBlock.Tools["CogFitCircle"] as CogFitCircleTool;
                cogDisplayStatusBarV21.Display = cogRecordDisplay1;
                cogDisplayToolbarV21.Display = cogRecordDisplay1;
                //  IniCelibEventHandler();
            }
            catch (Exception ex)
            {
                MessageBox.Show(ex.ToString());
            }

        }
        private void btn_calibration_Click(object sender, EventArgs e)
        {
            try
            {
                this.Cursor = Cursors.WaitCursor;

                //标定相机
                for (int i = 0; i < 9; i++)
                {
                    Calib_cel.Calibration.SetUncalibratedPointX(i, Cpras.NCD.Pixel_X[i]);
                    Calib_cel.Calibration.SetUncalibratedPointY(i, Cpras.NCD.Pixel_Y[i]);

                    //输入机械手坐标到标定工具中
                    Calib_cel.Calibration.SetRawCalibratedPointX(i, Cpras.NCD.Robot_X[i]);
                    Calib_cel.Calibration.SetRawCalibratedPointY(i, Cpras.NCD.Robot_Y[i]);

                }
                //  Calib_cel = GObj.Block_SNQ1Location.Tools["CalibPtoP"] as Cognex.VisionPro.CalibFix.CogCalibNPointToNPointTool;
                Calib_cel.Calibration.DOFsToCompute = CogNPointToNPointDOFConstants.ScalingAspectRotationSkewAndTranslation;
                Calib_cel.Calibration.Calibrate();
                Thread.Sleep(100);
                CogSerializer.SaveObjectToFile(CalibBlock, 外设.SysPras.CalVpppath);   //保存VPP
                Thread.Sleep(100);
                //GObj.VisionMth.LoadVPP(out MainForm.Block_SNQ1Location, Param.SNQ1VppFile);
                //GObj.VisionMth.LoadVPP(out GObj.Block_SNQ1Cal, Param.DownCalVppFile);
                MessageBox.Show("标定成功");

                #region【标定成功后将所有按钮的背景色还原】
                btn_getpixel1.BackColor = Color.Transparent;
                btn_getpixel2.BackColor = Color.Transparent;
                btn_getpixel3.BackColor = Color.Transparent;
                btn_getpixel4.BackColor = Color.Transparent;
                btn_getpixel5.BackColor = Color.Transparent;
                btn_getpixel6.BackColor = Color.Transparent;
                btn_getpixel7.BackColor = Color.Transparent;
                btn_getpixel8.BackColor = Color.Transparent;
                btn_getpixel9.BackColor = Color.Transparent;
                #endregion【标定成功后将所有按钮的背景色还原】
                this.Cursor = Cursors.Default;
            }
            catch (Exception ex)
            {
                this.Cursor = Cursors.Default;
                MessageBox.Show("标定失败" + ex.ToString());
            }
        }
        int j = 0;

        /// <summary>
        /// 自动标定流程
        /// //1.发送第一个点给机械手,机械手走到当前位,在把点位返回给上位机
        ///   2.串口接收数据,判断是第一个点位,处理标定算法
        ///   3.上位机发送第2个点位给机械手...
        /// </summary>
        /// <param name="sender"></param>
        /// <param name="e"></param>
        private void btn_AutoClib_Click(object sender, EventArgs e)
        {

        }

        /// <summary>
        /// 九点标定定时器
        /// </summary>
        /// <param name="sender"></param>
        /// <param name="e"></param>

        private void Calib(int i)
        {
            try
            {
                Thread.Sleep(300);
                cogRecordDisplay1.StaticGraphics.Clear();
                cogRecordDisplay1.InteractiveGraphics.Clear();
                #region【标定过程】

                //CogFitCircleTool FitCircle = GObj.Block_DownCal.Tools["CogFitCircle1"] as CogFitCircleTool;
                //先拍照
                FIFO_Acq.Run();
               // _GetOrgPix.Inputs["Image"].Value = FIFO_Acq.OutputImage;

                _GetOrgPix.Run();
                cogRecordDisplay1.Record = _GetOrgPix.CreateLastRunRecord().SubRecords[0];
                cogRecordDisplay1.Fit();
                Cpras.NCD.Pixel_X[i] = (double)_GetOrgPix.Outputs["X"].Value; //像素X坐标值
                Cpras.NCD.Pixel_Y[i] = (double)_GetOrgPix.Outputs["Y"].Value; //像素Y坐标值
                DG_Xpiexl.Rows[i].Cells[0].Value = _GetOrgPix.Outputs["X"].Value;  //像素X坐标值
                DG_Ypiexl.Rows[i].Cells[0].Value = _GetOrgPix.Outputs["Y"].Value;  //像素Y坐标值


                #endregion【标定选择】

                #region【写入标定VPP中的标定工具】
                //将得到的像素坐标赋值给标定工具对应点
                //CalibAll.Calibration.SetUncalibratedPointX(i, Cpras.NCD.Pixel_X[i]);
                //CalibAll.Calibration.SetUncalibratedPointY(i, Cpras.NCD.Pixel_Y[i]);
                ////输入机械手坐标到标定工具中
                //CalibAll.Calibration.SetRawCalibratedPointX(i, Cpras.NCD.Robot_X[i]);
                //CalibAll.Calibration.SetRawCalibratedPointY(i, Cpras.NCD.Robot_Y[i]);
                #endregion【写入标定VPP中的工具】

                #region【写入运行VPP中的标定工具】
                //将得到的像素坐标赋值给标定工具对应点
                Calib_cel.Calibration.SetUncalibratedPointX(i, Cpras.NCD.Pixel_X[i]);
                Calib_cel.Calibration.SetUncalibratedPointY(i, Cpras.NCD.Pixel_Y[i]);
                //输入机械手坐标到标定工具中
                Calib_cel.Calibration.SetRawCalibratedPointX(i, Cpras.NCD.Robot_X[i]);
                Calib_cel.Calibration.SetRawCalibratedPointY(i, Cpras.NCD.Robot_Y[i]);
                #endregion【写入运行VPP中的工具】

            }
            catch (Exception ex)
            {
                MessageBox.Show("像素坐标获取失败" + ex.ToString());
            }
        }
        private void btn_creatRobotMet_Click(object sender, EventArgs e)
        {
            for (int i = 0; i < 9; i++)
            {
                if (i < 3)
                {
                    DG_Xrobot.Rows[i].Cells[0].Value = Convert.ToDouble(tbx_sanpx.Text) + Convert.ToDouble(tbx_DistenceX.Text) * i;
                    DG_Yrobot.Rows[i].Cells[0].Value = Convert.ToDouble(tbx_sanpy.Text);

                }
                else if ((i >= 3) && (i < 6))
                {
                    DG_Xrobot.Rows[i].Cells[0].Value = Convert.ToDouble(tbx_sanpx.Text) + Convert.ToDouble(tbx_DistenceX.Text) * (5 - i);
                    DG_Yrobot.Rows[i].Cells[0].Value = Convert.ToDouble(tbx_sanpy.Text) - Convert.ToDouble(tbx_DistenceY.Text);
                }
                else
                {
                    DG_Xrobot.Rows[i].Cells[0].Value = Convert.ToDouble(tbx_sanpx.Text) + Convert.ToDouble(tbx_DistenceX.Text) * (i - 6);
                    DG_Yrobot.Rows[i].Cells[0].Value = Convert.ToDouble(tbx_sanpy.Text) - Convert.ToDouble(tbx_DistenceY.Text) * 2;
                }
            }

        }



        private void  getRobotzobiao(int i)
        {

            if(外设.rtuClient.IsConnect)
            {
                //外设.汇川PLC.ReadD(外设.汇川PLC.plcconfig._当前位置地址_x, out int robotx);
                //外设.汇川PLC.ReadD(外设.汇川PLC.plcconfig._当前位置地址_y, out int roboty);
                //DG_Xrobot.Rows[i].Cells[0].Value = robotx;
                //DG_Yrobot.Rows[i].Cells[0].Value = roboty;

            }
            else
            {
                MessageBox.Show("PLC未连接，请手动获取");
            }
           
        }
        private void btn_realdisplay_Click(object sender, EventArgs e)
        {
            try
            {
                FIFO_Acq.Run();
                cogRecordDisplay1.Record = FIFO_Acq.CreateLastRunRecord().SubRecords[0];
                cogRecordDisplay1.Fit();
                //if (btn_realdisplay.Text == "打开实时")
                //{
                //    btn_realdisplay.Text = "实时中";
                //    cogRecordDisplay1.StaticGraphics.Clear();
                //    cogRecordDisplay1.InteractiveGraphics.Clear();
                //    cogRecordDisplay1.StartLiveDisplay(FIFO_Acq.Operator, false);
                //    cogRecordDisplay1.Fit();
                //}
                //else
                //{
                //    btn_realdisplay.Text = "打开实时";
                //    cogRecordDisplay1.StopLiveDisplay();

                //}
            }
            catch (Exception EX)
            {

                System.Windows.Forms.MessageBox.Show("打开实时失败" + EX.Message);
            }
        }
        private void btn_saverobot_Click(object sender, EventArgs e)
        {
            try
            {

                Cpras.ACD.NTNCalibAP.x = Convert.ToDouble(tbx_sanpx.Text);
                Cpras.ACD.NTNCalibAP.y = Convert.ToDouble(tbx_sanpy.Text);
                Cpras.ACD.NTNCalibAP.r = Convert.ToDouble(tbx_sanpr.Text);
                Cpras.ACD.Xinterval = Convert.ToDouble(tbx_DistenceX.Text);
                Cpras.ACD.Yinterval = Convert.ToDouble(tbx_DistenceY.Text);
                for (int i = 0; i < 9; i++)
                {
                    Cpras.NCD.Robot_X[i] = Convert.ToDouble( DG_Xrobot.Rows[i].Cells[0].Value.ToString());
                    Cpras.NCD.Robot_Y[i] = Convert.ToDouble( DG_Yrobot.Rows[i].Cells[0].Value.ToString());
                }
            }
            catch (Exception ex)
            {
                MessageBox.Show("保存数据失败" + ex.Message);
            }
            //直接保存整个类
            string strerr = XmlHelper.SaveXML(SystemPras.Calibfile, Cpras);
            if (strerr != "")
            {
                MessageBox.Show(strerr);

            }
        }


        private void btn_AutoClibCenter_Click(object sender, EventArgs e)
        {

            try
            {
                #region【标定开始时计数及标志位复位】
                #endregion【标定开始时计数及标志位复位】
            }
            catch (Exception ex)
            {
                MessageBox.Show(ex.ToString());
            }
        }


        //计算旋转中心
        private void btn_CalculateCenter_Click(object sender, EventArgs e)
        {
            double X = 0.0, Y = 0.0, R = 0.0;
            //

            try
            {
                FitCircle.Run();
                cogRecordDisplay1.Record = FitCircle.CreateLastRunRecord().SubRecords[1];
                cogRecordDisplay1.Fit();
                X = FitCircle.Result.GetCircle().CenterX;
                Y = FitCircle.Result.GetCircle().CenterY;
                R = FitCircle.Result.GetCircle().Radius;  //工具到法兰中心的距离
                tbx_centerX.Text = X.ToString("F3");
                tbx_centerY.Text = Y.ToString("F3");
                tbx_radius.Text = R.ToString("F3");
            }
            catch (Exception ex)
            {
                MessageBox.Show(ex.Message);
            }
        }
        private void SetCorclePT(int i)
        {
            try
            {
                CalibBlock.Run();
                cogRecordDisplay1.Record = CalibBlock.CreateLastRunRecord().SubRecords[3];
                cogRecordDisplay1.Fit();
                double X = (double)CalibBlock.Outputs["CenterPTX"].Value;
                double Y = (double)CalibBlock.Outputs["CenterPTY"].Value;
                //这里要移动机械坐标，保存机械坐标

                //机械坐标
                FitCircle.RunParams.SetPoint(i, X, Y);
            }
            catch (Exception ex)
            {
                MessageBox.Show(ex.Message);
            }
        }

        private void btn_SetFitCirclePT_Click(object sender, EventArgs e)
        {
            try
            {
                if (((Button)sender).Text == "获取坐标1")
                {
                    SetCorclePT(0);
                    btn_SetFitCircle1.BackColor = Color.Green;
                }
                if (((Button)sender).Text == "获取坐标2")
                {
                    SetCorclePT(1);
                    btn_SetFitCircle2.BackColor = Color.Green;
                }
                if (((Button)sender).Text == "获取坐标3")
                {
                    SetCorclePT(2);
                    btn_SetFitCircle3.BackColor = Color.Green;
                }
                if (((Button)sender).Text == "获取坐标4")
                {
                    SetCorclePT(3);
                    btn_SetFitCircle4.BackColor = Color.Green;
                }
                if (((Button)sender).Text == "获取坐标5")
                {
                    SetCorclePT(4);
                    btn_SetFitCircle5.BackColor = Color.Green;
                }
                if (((Button)sender).Text == "获取坐标6")
                {
                    SetCorclePT(5);
                    btn_SetFitCircle6.BackColor = Color.Green;
                }
                if (((Button)sender).Text == "获取坐标7")
                {
                    SetCorclePT(6);
                    btn_SetFitCircle7.BackColor = Color.Green;
                }
                if (((Button)sender).Text == "获取坐标8")
                {
                    SetCorclePT(7);
                    btn_SetFitCircle8.BackColor = Color.Green;
                }
                if (((Button)sender).Text == "获取坐标9")
                {
                    SetCorclePT(8);
                    btn_SetFitCircle9.BackColor = Color.Green;
                }
            }
            catch (Exception ex)
            {
                MessageBox.Show("获取像素坐标异常" + ex.ToString());
            }

        }

        private void btn_SavecCenter_Click(object sender, EventArgs e)
        {

            Cpras.BPD.CircleRobotFitBP.x = Convert.ToDouble(tbx_centerX.Text);
            Cpras.BPD.CircleRobotFitBP.y = Convert.ToDouble(tbx_centerY.Text);
            //第一个点的机械坐标作为旋转中心的坐标
            Cpras.BPD.CircleRobotBP.x = Convert.ToDouble(DG_FitCRob_X.Rows[0].Cells[0].Value);
            Cpras.BPD.CircleRobotBP.y = Convert.ToDouble(DG_FitCRob_Y.Rows[0].Cells[0].Value);
            //直接保存整个类
            string strerr = XmlHelper.SaveXML(SystemPras.Calibfile, Cpras);
            if (strerr != "")
            {
                MessageBox.Show(strerr);

            }

        }
        private void btn_getpixel_Click(object sender, EventArgs e)
        {
            try
            {
                if (((Button)sender).Text == "获取坐标1")
                {
                    Calib(0);
                    btn_getpixel1.BackColor = Color.Green;
                }
                if (((Button)sender).Text == "获取坐标2")
                {
                    Calib(1);
                    btn_getpixel2.BackColor = Color.Green;
                }
                if (((Button)sender).Text == "获取坐标3")
                {
                    Calib(2);
                    btn_getpixel3.BackColor = Color.Green;
                }
                if (((Button)sender).Text == "获取坐标4")
                {
                    Calib(3);
                    btn_getpixel4.BackColor = Color.Green;
                }
                if (((Button)sender).Text == "获取坐标5")
                {
                    Calib(4);
                    btn_getpixel5.BackColor = Color.Green;
                }
                if (((Button)sender).Text == "获取坐标6")
                {
                    Calib(5);
                    btn_getpixel6.BackColor = Color.Green;
                }
                if (((Button)sender).Text == "获取坐标7")
                {
                    Calib(6);
                    btn_getpixel7.BackColor = Color.Green;
                }
                if (((Button)sender).Text == "获取坐标8")
                {
                    Calib(7);
                    btn_getpixel8.BackColor = Color.Green;
                }
                if (((Button)sender).Text == "获取坐标9")
                {
                    Calib(8);
                    btn_getpixel9.BackColor = Color.Green;
                }
            }
            catch (Exception ex)
            {
                MessageBox.Show("获取像素坐标异常" + ex.ToString());
            }

        }


        private void btn_SaveCelibVPP_Click(object sender, EventArgs e)
        {
            try
            {
                this.Cursor = Cursors.WaitCursor;
                CalibBlock = cogToolBlockEditV21.Subject;
                CogSerializer.SaveObjectToFile(CalibBlock, 外设.SysPras.CalVpppath);  //保存VPP
                                                                                    //GObj.VisionMth.LoadVPP(out GObj.Block_SNQ1Cal, Param.DownCalVppFile);   //加载VPP
                System.Threading.Thread.Sleep(200);
                this.Cursor = Cursors.Default;
                MessageBox.Show("标定VPP保存成功!");
            }
            catch (Exception ex)
            {
                MessageBox.Show(ex.Message);
            }



        }

        private void btn_SaveData_Click(object sender, EventArgs e)
        {
            string strerr = XmlHelper.SaveXML(SystemPras.Calibfile, Cpras);
            if (strerr != "")
            {
                MessageBox.Show(strerr);

            }
            else
            {
                MessageBox.Show("保存成功");
            }
        }

        private void btn_load_Click(object sender, EventArgs e)
        {
            CalibDataList tCpras = XmlHelper.LoadXML<CalibDataList>(SystemPras.Calibfile);
            if (tCpras == null)
            {
                MessageBox.Show("参数加载失败");
                return;
            }
            Cpras = tCpras;
            //标准参数
            SysGDV.SelectedObject = Cpras.BPD;
        }

        private void btn_保存旋转标定机械坐标_Click(object sender, EventArgs e)
        {
            //保存旋转坐标
            for (int i = 0; i < 9; i++)
            {
                Cpras.FCD.Robot_X[i] = Convert.ToDouble(DG_FitCRob_X.Rows[i].Cells[0].Value);
                Cpras.FCD.Robot_Y[i] = Convert.ToDouble(DG_FitCRob_Y.Rows[i].Cells[0].Value);
                Cpras.FCD.Robot_R[i] = Convert.ToDouble(DG_FitCRob_R.Rows[i].Cells[0].Value);
            }

            //保存
            string strerr = XmlHelper.SaveXML(SystemPras.Calibfile, Cpras);
            if (strerr != "")
            {
                MessageBox.Show(strerr);
            }

        }

        private void btn_获取实际1_Click(object sender, EventArgs e)
        {
            for(int i=1;i<10;i++)
            {
                if (((Button)sender).Text.Contains(i.ToString()))
                {
                    getRobotzobiao(i-1);
                }
            }
                    
        }
    }
}
